/********** Library Includes **************************************************/

#include <opencv/cv.h>
#include <opencv/highgui.h>

/********** Local Includes ****************************************************/

#ifdef WIN32
#include "livesource.h"
#else
#include "wacawsource.h"
#endif

#include "objectdetector.h"
#include "approxDecomp.h"
#include "common.h"
#include "celldecompastar.h"
#include "simpledrivecontroller.h"
#include "rovio.h"
#include "plan.h"

/********** Macros and Constants **********************************************/

#define DEBUG true
#define ROVIO_HOST "192.168.10.18"
#define TIME 500
#define REPLAN_THRESH 50

/********** Namespaces ********************************************************/

using namespace cv;
using namespace std;

/********** Local Functions ***************************************************/

Point robot(231, 241), fruit(662, 241);

void on_mouse(int event, int x, int y, int flags, void *)
{
	static int count = 0;

	switch(event) {
		case CV_EVENT_LBUTTONDOWN:
			if (count == 0) {
				robot = Point(x, y);
				cout << "Got robot position: (" << x << ", " << y << ")" << endl;
			} else if (count == 1) {
				fruit = Point(x, y);
				cout << "Got fruit position: (" << x << ", " << y << ")" << endl;
			}

			count++;
			break;
	}
}

void showPlan(const Mat &image, const vector<Point> plan, int blockSize, int next = -1) {
	Mat temp = image.clone();
	int count = 0;

	for (vector<Point>::const_iterator it = plan.begin(); it != plan.end(); ++it) {
		Point p(it->x * blockSize + blockSize / 2,
			it->y * blockSize + blockSize / 2);

		if (count == next) {
			circle(temp, p, 7, CV_RGB(0, 0, 255), CV_FILLED);
		} else {
			circle(temp, p, 5, CV_RGB(255, 0, 0), CV_FILLED);
		}

		count++;
		vector<Point>::const_iterator next = it + 1;

		if (next != plan.end()) {
			Point a(it->x * blockSize + blockSize / 2,
				it->y * blockSize + blockSize / 2);
			Point b(next->x * blockSize + blockSize / 2,
				next->y * blockSize + blockSize / 2);

			line(temp, a, b, CV_RGB(255, 0, 0));
		}
	}

	imshow("Plan", temp);
	cvMoveWindow("Plan", 0, 900 - temp.size().height);
}

int manhatten(cv::Point a, cv::Point b) {
	return abs(a.x - b.x) + abs(a.y - b.y);
}

int main(int, char **) {
	cout << "Press 'n' to step forward to the next frame." << endl
		<< "Press 'q' to close." << endl
		<< "In debug view:" << endl
		<< "\tRed is obstacle" << endl
		<< "\tGreen is fruit" << endl
		<< "\tBlue is robot" << endl;

	// I appologize in advance
	#ifdef WIN32
	LiveSource *source = new LiveSource();
	#else
	WacawSource *source = new WacawSource();
	#endif

	ObjectDetector detector(source);

	#ifdef WIN32
    source->next();
	#endif

	detector.saveBackground();

	imshow("Processed", detector.processed());
	imshow("Raw", source->last());

	cvMoveWindow("Processed", 0, 0);
	cvMoveWindow("Raw", source->last().size().width, 0);

	cout << "Place obstacles and press 'n'" << endl;

	while (cvWaitKey(300) != 'n');

    #ifdef WIN32
    source->next();
	#endif

	detector.saveObstacles();

	imshow("Processed", detector.processed());
	imshow("Raw", source->last());

	cout << "Place and robot and fruit and press 'n'" << endl;

	while (cvWaitKey(300) != 'n');

    #ifdef WIN32
    source->next();
	#endif

	imshow("Live", source->next());
	cvSetMouseCallback("Live", on_mouse, 0);

	cout << "Click near the robot and fruit (in that order), then press 'c'"
		<< endl;

	int key;

	while (true) {
		key = waitKey(300);

		if (key == 'q') {
			return 0;
		} else if (key == 'c') {
			break;
		}
	}

	#ifdef WIN32
	source->next();
	#endif

	cvDestroyWindow("Live");

	if (DEBUG) {
		cvNamedWindow("Robot Blobs");
		cvMoveWindow("Robot Blobs", source->last().size().width,
			900 - source->last().size().height);
	}

	detector.saveRobotAndFruit(robot, fruit, DEBUG);

    Mat open = detector.getOpen();
    ApproxDecomp decomp(source, open);

    Mat grid = decomp.cellDecomp();

	imshow("Processed", detector.processed());
	imshow("Raw", source->last());
	SimpleDriveController *controller = new SimpleDriveController();
	Rovio rovio(ROVIO_HOST);
	RobotOrientations current;


//REPLAN:

	Plan plan(grid);
	robot = detector.robot().point;
	fruit = detector.fruit();
	plan.buildPlan(robot, fruit, decomp.blockSize());

	showPlan(source->last(), plan.plan(), decomp.blockSize());
	cvWaitKey(1);
	DUMP(plan.plan());

	while (!plan.done()) {
		//int key = cvWaitKey(300);

		//if (key == 'q') {
		//	break;
		//}
		
		#ifdef WIN32
		source->next();
		#endif

//		RobotOrientation current = detector.findRobot(DEBUG);
		current = detector.findRobots();

		DUMP(current.rovio.point.x);
		DUMP(current.rovio.point.y);
		DUMP(current.pleo.x);
		DUMP(current.pleo.y);

		int dist = manhatten(current.pleo, plan.goal()*decomp.blockSize());
		DUMP(dist);

		if(dist > REPLAN_THRESH) {
            cout << "OMG IT'S TIME TO REPLAN!!!!" << endl;
            //goto REPLAN;
			plan.buildPlan(detector.robot().point, detector.fruit(),
				decomp.blockSize());
		}


		Command command;

		do {
			command = controller->next(current.rovio,
				plan.target() * decomp.blockSize());

			if (command.direction == DIR_STOP) {
				plan.advance();
				cout << "Moving on to next step" << endl;
			}
		} while (command.direction == DIR_STOP);

		showPlan(source->last(), plan.plan(), decomp.blockSize(), plan.step());

		imshow("Processed", detector.processed());
		imshow("Raw", source->last());

		waitKey(1);
		rovio.timeDrive(command.direction, command.time, command.speed);
		while(waitKey(100)!='p');

	}
}
